#include <geometry_msgs/msg/point_stamped.hpp>
#include <message_filters/subscriber.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer.hpp>
#include <tf2_ros/create_timer_ros.hpp>
#include <tf2_ros/message_filter.hpp>
#include <tf2_ros/transform_listener.hpp>

using namespace std::chrono_literals;

// 坐标点的变换监听节点
class ListenerMsgFilter : public rclcpp::Node {
public:
  // 构造函数
  ListenerMsgFilter() : Node("tf_listener") {

    // 已启动消息筛选器的侦听器
    RCLCPP_INFO(this->get_logger(), "已启动消息筛选器的侦听器.");

    // 创建坐标变换缓存
    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());

    // 定时器timer - 使用正确的类名 tf2_ros::CreateTimerROS
    timer_ = std::make_shared<tf2_ros::CreateTimerROS>(
        this->get_node_base_interface(), this->get_node_timers_interface());

    tf_buffer_->setCreateTimerInterface(timer_);

    // 创建坐标变换监听器
    tf_listener_ =
        std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, this);

    // 订阅坐标点
    point_sub_.subscribe(this, "point", rclcpp::QoS(10));

    // 创建消息过滤器
    tf_filter_ = std::make_shared<
        tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>(
        point_sub_, *tf_buffer_, "base_link", 10,
        this->get_node_logging_interface(), this->get_node_clock_interface(),
        1s);

    // 注册回调函数
    tf_filter_->registerCallback(
        std::bind(&ListenerMsgFilter::pointCB, this, std::placeholders::_1));
  }

private:
  void
  pointCB(const geometry_msgs::msg::PointStamped::ConstSharedPtr point_msg) {
    // 输出接收到的坐标点
    RCLCPP_INFO(this->get_logger(),
                "输出接收到 的坐标点 (%.2f, %.2f, %.2f) in frame %s",
                point_msg->point.x, point_msg->point.y, point_msg->point.z,
                point_msg->header.frame_id.c_str());

    // 定义变换后的坐标点
    geometry_msgs::msg::PointStamped point_out;
    // 捕获变换异常
    try {
      // 坐标变换
      tf_buffer_->transform(*point_msg, point_out, "base_link");

      // 成功变换
      RCLCPP_INFO(this->get_logger(),
                  "成功变换 的坐标点(%.2f, %.2f, %.2f) in frame %s",
                  point_out.point.x, point_out.point.y, point_out.point.z,
                  point_out.header.frame_id.c_str());
    } catch (tf2::TransformException &ex) {
      // 变换失败
      RCLCPP_WARN(this->get_logger(), "Transform failed: %s", ex.what());
    }
  }

  // 定时器 timer
  std::shared_ptr<tf2_ros::CreateTimerInterface> timer_;

  // 坐标变换缓存
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  // 坐标变换监听器
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
  // 坐标点订阅器
  message_filters::Subscriber<geometry_msgs::msg::PointStamped> point_sub_;
  // 消息过滤器
  std::shared_ptr<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>
      tf_filter_;
};

// 坐标点的变换监听

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<ListenerMsgFilter>());
  rclcpp::shutdown();
  return 0;
}
